/**
 * Sensors.java
 *
 * This uses portions of code from the SendDataDemo included with the Java
 * SunSPOT SDK.
 */

package RoomNanny.SpotSensors;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ITemperatureInput;
import com.sun.spot.sensorboard.peripheral.IAccelerometer3D;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ISwitchListener;
import com.sun.spot.sensorboard.io.IScalarInput;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.peripheral.radio.RadioFactory;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.io.j2me.radiostream.*;
import com.sun.spot.io.j2me.radiogram.*;
import com.sun.spot.util.*;

import java.io.*;
import javax.microedition.io.*;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * This is a midlet application which will run on a SunSPOT. The application
 * will poll its sensors every fifteen minutes, then send that data wirelessly
 * to a SunSPOT basestation listening on the same port.
 */
public class Sensors extends MIDlet implements ISwitchListener {

    private static final int HOST_PORT = 67;
    private static final int SAMPLE_PERIOD = 500;  // in milliseconds

    private int id = -1;
    private int ledMode = 0;

    // Sensorboard initialization
    private ITemperatureInput tempSensor = EDemoBoard.getInstance().getADCTemperature();
    private IScalarInput lightSensor = EDemoBoard.getInstance().getLightSensor();
    private ITriColorLED[] leds = EDemoBoard.getInstance().getLEDs();
    private IAccelerometer3D accelerometer = EDemoBoard.getInstance().getAccelerometer();
    private ISwitch switch1 = EDemoBoard.getInstance().getSwitches()[0];
    private ISwitch switch2 = EDemoBoard.getInstance().getSwitches()[1];

    private long lastTime = 0L;
    private static final long sample = 100;

    private long currentTime = 0L; // time of reading
    private double tempReading = 0;  // temperature at time of reading
    private int lightReading = 0; // light value
    // Accelerometer recordings
    private double accelReadingX = 0;
    private double accelReadingY = 0;
    private double accelReadingZ = 0;

    // Light calibration
    private int lightMin;
    private int lightMax;
    private boolean isLightOn;

    /// TODO: Device needs to set real values for these on start up
    private double reportedTemp = 0;
    private boolean reportedLightOn = false;

    private RadiogramConnection outConnection = null; // Radio connection to send over
    private RadiogramConnection inConnection = null;
    private Datagram dgOut = null; // Packet to hold outgoing data
    private Datagram dgIn = null;

    private String hardwareAddress;

    /**
     * This method is called when the SunSPOT starts. It contains the main
     * program loop, whose primary functionality is to poll the sensors and
     * send the data to the base station.
     */
    protected void startApp() throws MIDletStateChangeException {


        hardwareAddress = System.getProperty("IEEE_ADDRESS"); // spot address

        System.out.println("Starting sensor sampler application on " +
                hardwareAddress + " ...");

        // Listen for downloads/commands over USB connection
        new com.sun.spot.util.BootloaderListener().start();

        switch1.addISwitchListener(this);   // listen for input on first switch

        try {
            // Open up a broadcast connection to the host port
            // where the 'on Desktop' portion of this demo is listening
            outConnection = (RadiogramConnection) Connector.open("radiogram://broadcast:" + HOST_PORT);
            dgOut = outConnection.newDatagram(50);  // only sending 12 bytes of data
            // Open up connection to receive data sent by the base station
            inConnection = (RadiogramConnection) Connector.open("radiogram://:" + HOST_PORT);
            dgIn = inConnection.newDatagram(50);
        } catch (Exception e) {
            System.err.println("Caught " + e + " in connection initialization.");
            System.exit(1);
        }

        // Turn connectivity status LED to indicate device has not yet been assigned an ID
        //leds[7].setRGB(255, 0, 0);
        //leds[7].setOn();
        /// TODO: Attempt to fetch ID more than once if failed the first time.
        // Get device ID
        leds[7].setRGB(255, 0, 0);
        leds[7].setOn();
        id = getNewID(); // Will not return until appropriate id is received.
        leds[7].setOff();

        /*
        try {
            // Get initial sensor readings
            reportedTemp = tempSensor.getFahrenheit();
            reportedLightOn = lightSensor.getValue(); // Needs to be converted to boolean... relies on calibration!
        } catch (Exception e) {
            System.out.println("Caught " + e +
                               " while getting initial sensor values");
        }
         */

        double a;
        // TODO: While loop should be separate method.
        while (true) {
            // Start sampling period
            leds[5].setRGB(255, 255, 255);
            leds[5].setOn();

            // Main program loop
            try {
                currentTime = System.currentTimeMillis();
                // Get sensor values
                tempReading = tempSensor.getFahrenheit();
                lightReading = lightSensor.getValue();
                accelReadingX = accelerometer.getAccelX();
                accelReadingY = accelerometer.getAccelY();
                accelReadingZ = accelerometer.getAccelZ();
            } catch (Exception e) {
                System.err.println("Caught " + e +
                        " while collecting/sending sensor sample.");
            }

            switch (ledMode) {
                case 0:
                    // Mode 0 - Display Status LEDS
                    // If light is above the halfway mark, light is on, otherwise it's off
                    if (lightReading > Math.abs(lightMax - lightMin)/2 + lightMin) {
                        // Light is on
                        leds[6].setRGB(0, 255, 0); // Set green
                        leds[6].setOn();
                        isLightOn = true;
                    } else {
                        // Light is off
                        leds[6].setRGB(0, 0, 255); // Set blue
                        leds[6].setOn();
                        isLightOn = false;
                    }

                    // Calculate overall acceleration
                    /*
                    a = Math.sqrt(accelReadingX*accelReadingX
                            + accelReadingY*accelReadingY
                            + accelReadingZ*accelReadingZ);
                    //if (accelReadingZ > 1) {
                    if (a > 1.05) {
                        leds[0].setRGB(255, 255, 255);
                        leds[0].setOn();
                    } else {
                        leds[0].setOff();
                    }
                    */

                    // Check for events

                    // Compare light status to last reported light status
                    if (isLightOn != reportedLightOn) {
                        // Send data, update reported data
                        sendData();

                    // Compare current temperature with last reported temp reading
                    } else if (Math.abs(tempReading - reportedTemp) >= 1.0) {
                        // Send data, update reported data
                        sendData();
                    }
                    break;
                case 1:
                    // Mode 1 - Display binary representation of ID on the LEDs
                    char[] binaryID = Integer.toBinaryString(id).toCharArray();

                    for (int i = 0; i < binaryID.length; i++) {
                        if (binaryID[i] == '1') {
                            leds[(leds.length - binaryID.length) + i].setRGB(255, 255, 255);
                            leds[(leds.length - binaryID.length) + i].setOn();
                        }
                    }
                    break;
            }

            // Start calibration
            if (switch2.isClosed())
            {
                calibrateLight();
            }

            leds[5].setOff(); // Sampling over

            // Go to sleep to conserve battery
            /*Utils.sleep(SAMPLE_PERIOD - (System.currentTimeMillis() -
                    currentTime));*/
            Utils.sleep(SAMPLE_PERIOD);
        }
    }

    /*
     * Calibrate lights based on range
     *
     */
    private void calibrateLight() {
        // Should be its own method later
        long lightCalibrationStart = System.currentTimeMillis();

        // Give inital values to the min and max
        lightMax = 0;
        lightMin = 255;

        // Indicate that we are calibrating
        leds[6].setRGB(255, 0, 0);
        leds[6].setOn();

        // Calibrate for 5 seconds...
        while ((lightCalibrationStart + 5000) > System.currentTimeMillis())
        //for (int i = 0; i < 50; i++)
        {
            try {
                lightReading = lightSensor.getValue();
            } catch (Exception e) {
                System.err.println("Caught " + e +
                        " while collecting/sending sensor sample.");
            }

            // Update bounds
            if (lightReading > lightMax)
            {
                lightMax = lightReading;
            }
            if (lightReading < lightMin)
            {
                lightMin = lightReading;
            }

            //Utils.sleep(100);
        }

        // Indicate that we are done calibrating
        leds[6].setOff();
    }

    /*
     * Send data packet containing sample data
     */
    private void sendData() {
        // Indicate that the packet has been sent
        // (Only if in appropriate led mode)
        if (ledMode == 0) {
            leds[7].setRGB(255, 255, 255);
            leds[7].setOn();
        }
        // Send our radiogram data package
        try {
            dgOut.reset();

            dgOut.writeShort(0);    // indicate that this is a data packet
            dgOut.writeInt(id);     // write ID number
            dgOut.writeLong(currentTime);
            dgOut.writeDouble(tempReading);
            dgOut.writeBoolean(isLightOn);
            /// TODO: Replace acceleration readings with door movement boolean
            dgOut.writeDouble(accelReadingX);
            dgOut.writeDouble(accelReadingY);
            dgOut.writeDouble(accelReadingZ);

            outConnection.send(dgOut);
        } catch (Exception e) {
            System.err.println("Caught exception " + e + "while sending data");
        }

        // Update last reported values
        reportedLightOn = isLightOn;
        reportedTemp = tempReading;

        // Turn off LED indicator now that data has been sent
        if (ledMode == 0) {
            leds[7].setOff();
        }
    }

    /**
     * Prints the sensor readings, great for debugging(TM)!
     */
    private void displayReadings() {
        System.out.println("Current temp = " + tempReading
                + " light = " + lightReading
                + " accel X = " + accelReadingX
                + " accel Y = " + accelReadingY
                + " accel Z = " + accelReadingZ);
    }

    /**
     * Switch callback function for when the switch button is released.
     *
     * @param sw The switch being released
     */
    public void switchReleased(ISwitch sw) {
        if (ledMode < 1) {
            ledMode = ledMode + 1;
        } else {
            ledMode = 0;
        }
        for (int i = 0; i < leds.length; i++) {
            leds[i].setOff();
        }
    }

    /*
     * Switch callback function for when the switch button is pressed.
     *
     * @param sw The switch being pressed.
     */
    public void switchPressed(ISwitch sw) {

    }

    /*
     * Get new device ID. Will not terminate until an address is received.
     */
    private int getNewID() {
        // Must get new ID number from the basestation
        int newID = -1;

        while (newID == -1) {
            try {
                dgOut.reset();
                dgOut.writeShort(1);   // indicate that this is a request for ID
                outConnection.send(dgOut);
                inConnection.receive(dgIn);
                // Only assign the id received if is the correct hardware addr
                if (dgIn.readUTF().equals(hardwareAddress)) {
                    newID = dgIn.readInt();
                    System.out.println("Got ID from basestation: " + newID);
                    inConnection.close();
                }
            } catch (Exception e) {
                System.err.println("Caught exception " + e + "while getting " +
                                   "request for ID number");
            }
        }
        return newID;
    }



    protected void pauseApp() {
        // This will never be called by the Squawk VM
    }


    protected void destroyApp(boolean arg0) throws MIDletStateChangeException {
        // Only called if startApp throws any exception other
        // than MIDletStateChangeException
    }
}